#include "DiffMuscle1.h"
#include <OpenSim/Common/SimmMacros.h>
#include <OpenSim/Common/DebugUtilities.h>
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Common/NaturalCubicSpline.h>
#include <iostream>

using namespace std;
using namespace OpenSim;


//=============================================================================

// PLUGIN - SPECIFIC FUNCTIONS

//=============================================================================

//_____________________________________________________________________________

/**

 * Perform some set up functions that happen after the

 * object has been deserialized or copied.

 * @param aModel model containing this DiffMuscle1.

 */

const string DiffMuscle1::STATE_SLOW_ACTIVATION_NAME = "slow_activation";

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 */
DiffMuscle1::DiffMuscle1() :
   ActivationFiberLengthMuscle()
{
	setNull();
	setupProperties();
}

//_____________________________________________________________________________
/**
 * Constructor.
 */
DiffMuscle1::DiffMuscle1(const std::string &aName,double aMaxIsometricForce,double aOptimalFiberLength,double aTendonSlackLength,double aPennationAngle) :
   ActivationFiberLengthMuscle()
{
	setNull();
	setupProperties();
	setName(aName);
	setMaxIsometricForce(aMaxIsometricForce);
	setOptimalFiberLength(aOptimalFiberLength);
	setTendonSlackLength(aTendonSlackLength);
	setPennationAngleAtOptimalFiberLength(aPennationAngle);
}

//_____________________________________________________________________________
/**
 * Destructor.
 */
DiffMuscle1::~DiffMuscle1()
{
}

//_____________________________________________________________________________
/**
 * Copy constructor.
 *
 * @param aMuscle DiffMuscle1 to be copied.
 */
DiffMuscle1::DiffMuscle1(const DiffMuscle1 &aMuscle) :
   ActivationFiberLengthMuscle(aMuscle)
{
	setNull();
	setupProperties();
	copyData(aMuscle);
}

//_____________________________________________________________________________
/**
 * Copy this muscle point and return a pointer to the copy.
 * The copy constructor for this class is used.
 *
 * @return Pointer to a copy of this DiffMuscle1.
 */
Object* DiffMuscle1::copy() const
{
	DiffMuscle1 *musc = new DiffMuscle1(*this);
	return(musc);
}

//=============================================================================
// CONSTRUCTION METHODS
//=============================================================================
//_____________________________________________________________________________
/**
 * Copy data members from one DiffMuscle1 to another.
 *
 * @param aMuscle DiffMuscle1 to be copied.
 */
void DiffMuscle1::copyData(const DiffMuscle1 &aMuscle)
{
	setPropertyValue("Vmax0FastFiber", aMuscle.getPropertyValue<double>("Vmax0FastFiber"));
	setPropertyValue("Vmax0SlowFiber", aMuscle.getPropertyValue<double>("Vmax0SlowFiber"));
	setPropertyValue("AfFastFiber", aMuscle.getPropertyValue<double>("AfFastFiber"));
	setPropertyValue("AfSlowFiber", aMuscle.getPropertyValue<double>("AfSlowFiber"));
	setPropertyValue("Flen", aMuscle.getPropertyValue<double>("Flen"));
	setPropertyValue("cScaleFastFiber", aMuscle.getPropertyValue<double>("cScaleFastFiber"));
	setPropertyValue("cScaleSlowFiber", aMuscle.getPropertyValue<double>("cScaleSlowFiber"));
	setPropertyValue("active_force_length_curve", aMuscle.getPropertyValue<Function *>("active_force_length_curve"));
	setPropertyValue("passive_force_length_curve", aMuscle.getPropertyValue<Function *>("passive_force_length_curve"));
}

//_____________________________________________________________________________
/**
 * Set the data members of this DiffMuscle1 to their null values.
 */
void DiffMuscle1::setNull()
{
	setType("DiffMuscle1");
}

//_____________________________________________________________________________
/**
 * Connect properties to local pointers.
 */
void DiffMuscle1::setupProperties()
{
	addProperty<double>("Vmax0FastFiber",
		"maximum intrinsic speed of fast fiber",
		5.0);
	
	addProperty<double>("Vmax0SlowFiber",
		"maximum intrinsic speed of slow fiber",
		3.0);

	addProperty<double>("AfFastFiber",
		"force-velocity K shape factor of fast fiber",
		0.29);

	addProperty<double>("AfSlowFiber",
		"force-velocity K shape factor of slow fiber",
		0.18);
	
	addProperty<double>("Flen",
		"maximum normalized lengthening force",
		1.8);

	addProperty<double>("cScaleFastFiber",
		"scales the relative contribution of fast fiber",
		1.8);

	addProperty<double>("cScaleSlowFiber",
		"scales the relative contribution of slow fiber",
		1.8);

	int activeForceLengthCurvePoints = 21;
	double activeForceLengthCurveX[] = {-5.30769200, -4.30769200, -1.92307700, -0.88461500, -0.26923100,  0.23076900,  0.46153800,  0.52725000,  0.62875000,  0.71875000,  0.86125000,  1.04500000,  1.21750000,  1.43875000,  1.50000000,  1.61538500,  2.00000000,  2.96153800,  3.69230800,  5.46153800,  9.90190200};
	double activeForceLengthCurveY[] = {0.01218800,  0.02189900,  0.03646600,  0.05249300,  0.07531200,  0.11415800,  0.15785900,  0.22666700,  0.63666700,  0.85666700,  0.95000000,  0.99333300,  0.77000000,  0.24666700,  0.19382100,  0.13325200,  0.07268300,  0.04441700,  0.03634100,  0.02189900,  0.00733200};
	NaturalCubicSpline *activeForceLengthCurve = new NaturalCubicSpline(activeForceLengthCurvePoints, activeForceLengthCurveX, activeForceLengthCurveY);
	addProperty<Function *>("active_force_length_curve",
		"Function representing active force-length behavior of muscle fibers",
		activeForceLengthCurve);

	int passiveForceLengthCurvePoints = 13;
	double passiveForceLengthCurveX[] = {-5.00000000,  0.99800000,  0.99900000,  1.00000000,  1.10000000,  1.20000000,  1.30000000,  1.40000000,  1.50000000,  1.60000000,  1.60100000,  1.60200000,  5.00000000};
	double passiveForceLengthCurveY[] = {0.00000000,  0.00000000,  0.00000000,  0.00000000,  0.03500000,  0.12000000,  0.26000000,  0.55000000,  1.17000000,  2.00000000,  2.00000000,  2.00000000,  2.00000000};
	NaturalCubicSpline *passiveForceLengthCurve = new NaturalCubicSpline(passiveForceLengthCurvePoints, passiveForceLengthCurveX, passiveForceLengthCurveY);
	addProperty<Function *>("passive_force_length_curve",
		"Function representing passive force-length behavior of muscle fibers",
		passiveForceLengthCurve);
}

//=============================================================================
// OPERATORS
//=============================================================================
//_____________________________________________________________________________
/**
 * Assignment operator.
 *
 * @return Reference to this object.
 */
DiffMuscle1& DiffMuscle1::operator=(const DiffMuscle1 &aMuscle)
{
	// BASE CLASS
	ActivationFiberLengthMuscle::operator=(aMuscle);
	copyData(aMuscle);

	return(*this);
}


//-----------------------------------------------------------------------------
// max intrinsic speed of fast fiber
//-----------------------------------------------------------------------------
bool DiffMuscle1::setVmax0FastFiber(double aVmax0)
{
	setPropertyValue("Vmax0FastFiber", aVmax0);
	return true;
}


//-----------------------------------------------------------------------------
// Set max intrinsic speed of slow fiber
//-----------------------------------------------------------------------------
bool DiffMuscle1::setVmax0SlowFiber(double aVmax0)
{
	setPropertyValue("Vmax0SlowFiber", aVmax0);
	return true;
}


int DiffMuscle1::getStateVariableYIndex(int index) const

{
       if (index==0)

             return _model->getMultibodySystem().getDefaultState().getZStart();
       if (index ==1)

             return _model->getMultibodySystem().getDefaultState().getZStart()+1;

       throw Exception("Trying to get Coordinate State variable YIndex for Coorindate "+getName()+" at undefined index"); 
}

bool DiffMuscle1::setScaleFastFiber(double cFastFiber)
{
	setPropertyValue("cScaleFastFiber", cFastFiber);
	return true;
}

bool DiffMuscle1::setScaleSlowFiber(double cSlowFiber)
{
	setPropertyValue("cScaleSlowFiber", cSlowFiber);
	return true;
}

double  DiffMuscle1::computeActuation(const SimTK::State& s) const
{
	const MuscleLengthInfo& mli = getMuscleLengthInfo(s);
	const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s);

	double tendonForce = getMaxIsometricForce();
	setForce(s, tendonForce);

	return tendonForce;
}


//_____________________________________________________________________________

/**

 * @param aNormFiberLength Normalized length of the muscle fiber.

 * @return The active force in the muscle fibers.

 */

double DiffMuscle1::calcActiveForceFastFiber(const SimTK::State& s, double aNormFiberLength) const
{
	return calcActiveForce(s, aNormFiberLength);
}

double DiffMuscle1::calcActiveForceSlowFiber(const SimTK::State& s, double aNormFiberLength) const

{
	return calcActiveForce(s, aNormFiberLength);
}


//=============================================================================
// CALCULATIONS
//=============================================================================
/* calculate muscle's position related values such fiber and tendon lengths,
	normalized lengths, pennation angle, etc... */
void DiffMuscle1::calcMuscleLengthInfo(const SimTK::State& s, MuscleLengthInfo& mli) const
{
	double norm_muscle_tendon_length = getLength(s) / getOptimalFiberLength();
	
	mli.fiberLength = getStateVariable(s, STATE_FIBER_LENGTH_NAME);
	
	if (mli.fiberLength < SimTK::Eps)
		mli.pennationAngle = 0.0;
	else{
		double value = getOptimalFiberLength() * sin(getPennationAngleAtOptimalFiberLength()) / mli.fiberLength;
		if ( isnan(value)  ) 
			mli.pennationAngle = 0.0;
		else if (value <= 0.0 )
			mli.pennationAngle = 0.0;
		else if (value >= 1.0)
			mli.pennationAngle = SimTK_PI/2.0;
		else
			mli.pennationAngle = asin(value);
	}

	mli.cosPennationAngle = cos(mli.pennationAngle);
	mli.tendonLength = getLength(s)-mli.fiberLength*mli.cosPennationAngle;
	
	mli.normFiberLength = mli.fiberLength/getOptimalFiberLength();
	mli.normTendonLength = norm_muscle_tendon_length - mli.normFiberLength * mli.cosPennationAngle;
	mli.tendonStrain = (mli.tendonLength/getTendonSlackLength()-1.0);
}

/* calculate muscle's velocity related values such fiber and tendon velocities,
	normalized velocities, pennation angular velocity, etc... */
void DiffMuscle1::calcFiberVelocityInfo(const SimTK::State& s, FiberVelocityInfo& fvi) const
{
	const MuscleLengthInfo& mli = getMuscleLengthInfo(s);
	const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s);

	const double &optimalFiberLength = getOptimalFiberLength();

	// Maximum contraction velocity is an activation scaled value
	double Vmax = getPropertyValue<double>("Vmax");
	const double &vmax0 = getPropertyValue<double>("Vmax0");
	if (mdi.activation < 1.0) {
		Vmax = vmax0 + mdi.activation*(Vmax-vmax0);
	}
	Vmax = Vmax*optimalFiberLength;

	/* If pennation equals 90 degrees, fiber length equals muscle width and fiber
	* velocity goes to zero.  Pennation will stay at 90 until tendon starts to
	* pull, then "stiff tendon" approximation is used to calculate approximate
	* fiber velocity.
	*/
	if (abs(mli.cosPennationAngle) < SimTK::SqrtEps) {
		if (abs(mdi.normTendonForce) < SimTK::SqrtEps) {
			fvi.normFiberVelocity = fvi.fiberVelocity = 0.0;
		} else { // assume rigid tendon
			fvi.fiberVelocity = getLengtheningSpeed(s);
			fvi.normFiberVelocity = fvi.fiberVelocity/(optimalFiberLength*getMaxContractionVelocity());
		}
	} else {
		//double velocity_dependent_force = mdi.normTendonForce / mli.cosPennationAngle - mdi.passiveForceMultiplier;
		//fvi.normFiberVelocity = calcFiberVelocity(s, mdi.activation, mdi.forceLengthMultiplier, velocity_dependent_force);
		fvi.fiberVelocity = fvi.normFiberVelocity * Vmax;
	}
}

/* calculate muscle's active and passive force-length, force-velocity, 
	tendon force, relationships and their related values */
void DiffMuscle1::calcMuscleDynamicsInfo(const SimTK::State& s, MuscleDynamicsInfo& mdi) const
{
	const MuscleLengthInfo &mli = getMuscleLengthInfo(s);
	const double &maxIsometricForce = getMaxIsometricForce();

	mdi.normTendonForce = calcTendonForce(s, mli.normTendonLength);
	//mdi.passiveForceMultiplier = calcPassiveForce(s, mli.normFiberLength);
	//mdi.passiveFiberForce = mdi.passiveForceMultiplier * maxIsometricForce;
	
	//mdi.forceLengthMultiplier = calcActiveForce(s, mli.normFiberLength);
	
	mdi.activation = getStateVariable(s, STATE_ACTIVATION_NAME);

	double tendonForce = maxIsometricForce*mdi.normTendonForce;
	mdi.activeFiberForce =  tendonForce/mli.cosPennationAngle - mdi.passiveFiberForce;

	//mdi.forceVelocityMultiplier = mdi.activeFiberForce/(maxIsometricForce *mdi.activation*mdi.forceLengthMultiplier);
}


//_____________________________________________________________________________
/**
 * @param aNormFiberLength Normalized length of the muscle fiber.
 * @return The passive force in the muscle fibers.
 */
double DiffMuscle1::calcPassiveForce(const SimTK::State& s, double aNormFiberLength) const
{
	return  getPassiveForceLengthCurve()->calcValue(SimTK::Vector(1, aNormFiberLength) );;
}

/*_____________________________________________________________________________
 * @param aNormFiberLength Normalized length of the muscle fiber.
 * @return The active force in the muscle fibers.
 */
double DiffMuscle1::calcActiveForce(const SimTK::State& s, double aNormFiberLength) const
{
	return getActiveForceLengthCurve()->calcValue(SimTK::Vector(1, aNormFiberLength));
}

//_____________________________________________________________________________
/**
 * @param aActivation Activation of the muscle.
 * @param aActiveForce Active force in the muscle fibers.
 * @param aVelocityDependentForce Force value that depends on fiber velocity.
 * @return The velocity of the muscle fibers.
 */
double DiffMuscle1::calcFiberVelocity(const SimTK::State& s, double aActivation, double aActiveForce, double aVelocityDependentForce) const
{
	double epsilon=1.e-6;

	// Don't allow zero activation
	if (aActivation<epsilon) 
		aActivation=epsilon;

	double Fa = aActivation*aActiveForce;
	double Fv = aVelocityDependentForce;

	const double &af = getPropertyValue<double>("Af");
	const double &flen = getPropertyValue<double>("Flen");

	double norm_fiber_velocity;
	if (Fv<Fa) {		// Muscle shortening
		if (Fv<0) {	// Extend the force-velocity curve for negative forces using linear extrapolation
			double F0=0;
			double b=Fa+F0/af;
        	double fv0 = (F0-Fa)/(b);
			double F1=epsilon;
			b=Fa+F1/af;
        	double fv1 = (F1-Fa)/(b);
			b = (F1-F0)/(fv1-fv0);
        	norm_fiber_velocity = fv0 + (Fv-F0)/b;
		}
		else {
			double b=Fa+Fv/af;
			norm_fiber_velocity = (Fv-Fa)/(b);
		}
	}
	else if (Fv<(.95*Fa*flen)) {
		double b=(2+2./af)*(Fa*flen-Fv)/(flen-1.);
		norm_fiber_velocity = (Fv-Fa)/(b);
	}
	else {  // Extend the force-velocity curve for forces that exceed maximum using linear extrapolation
		double F0=.95*Fa*flen;
		double b=(2+2./af)*(Fa*flen-F0)/(flen-1.);
		double fv0 = (F0-Fa)/(b);
		double F1=(.95+epsilon)*Fa*flen;
		b=(2+2./af)*(Fa*flen-F1)/(flen-1.);
		double fv1 = (F1-Fa)/(b);
		b = (fv1-fv0)/(F1-F0);
		norm_fiber_velocity = fv0 + b*(Fv-F0);
	}

	return norm_fiber_velocity;
}

//_____________________________________________________________________________
/**
 * Find the force produced by an actuator (the musculotendon unit), assuming
 * static equilibrium. Using the total muscle-tendon length, it finds the
 * fiber and tendon lengths so that the forces in each match. This routine
 * takes pennation angle into account, so its definition of static equilibrium
 * is when tendon_force = fiber_force * cos(pennation_angle). This funcion
 * will modify the object's values for length, fiberLength, activeForce, 
 * and passiveForce.
 *
 * @param aActivation Activation of the muscle.
 * @return The isometric force in the muscle.
 */
double DiffMuscle1::computeIsometricForce(SimTK::State& s, double aActivation) const
{
#define MAX_ITERATIONS 100
#define ERROR_LIMIT 0.01

    int i;
    double length,tendon_length, fiber_force, tmp_fiber_length, min_tendon_stiffness;
    double cos_factor, fiber_stiffness;
    double old_fiber_length, length_change, tendon_stiffness, percent;
    double error_force = 0.0, old_error_force, tendon_force, norm_tendon_length;
    double passiveForce;
    double fiberLength;

    const double &maxIsometricForce = getPropertyValue<double>("max_isometric_force");
    const double &optimalFiberLength = getPropertyValue<double>("optimal_fiber_length");
	const double &tendonSlackLength = getPropertyValue<double>("tendon_slack_length");
	const double &pennationAngleAtOptimal = getPropertyValue<double>("pennation_angle_at_optimal");

    if (optimalFiberLength < ROUNDOFF_ERROR) {
       setStateVariable(s, STATE_FIBER_LENGTH_NAME, 0.0);
       setForce(s, 0.0);
       return 0.0;
    }

	length = getLength(s);

    double muscle_width = _muscleWidth;

	if (tendonSlackLength < ROUNDOFF_ERROR) {
		tendon_length = 0.0;
		cos_factor = cos(atan(muscle_width / length));
		fiberLength = length / cos_factor;

		double activeForce = calcActiveForce(s, fiberLength / optimalFiberLength)  * aActivation;
		if (activeForce < 0.0) activeForce = 0.0;

		passiveForce = calcPassiveForce(s, fiberLength / optimalFiberLength);
		if (passiveForce < 0.0) passiveForce = 0.0;

		setStateVariable(s, STATE_FIBER_LENGTH_NAME, fiberLength);
		tendon_force = (activeForce + passiveForce) * maxIsometricForce * cos_factor;
		setForce(s, tendon_force);
		return tendon_force;
   } else if (length < tendonSlackLength) {
      setStateVariable(s, STATE_FIBER_LENGTH_NAME, muscle_width);
      _model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);
      setForce(s, 0.0);
      return 0.0;
   } else {
      fiberLength = optimalFiberLength;
      tendon_length = length - sqrt(fiberLength*fiberLength-_muscleWidth*_muscleWidth);
	  cos_factor = cos(atan(muscle_width / fiberLength));

      /* Check to make sure tendon is not shorter than its slack length. If it
       * is, set the length to its slack length and re-compute fiber length.
       */

      if (tendon_length < tendonSlackLength) {
         tendon_length = tendonSlackLength;
         cos_factor = cos(atan(muscle_width / (length - tendon_length)));
         fiberLength = (length - tendon_length) / cos_factor;
         if (fiberLength < muscle_width)
           fiberLength = muscle_width;
      }

   }

   // Muscle-tendon force is found using an iterative method. First, you guess
   // the length of the muscle fibers and the length of the tendon, and
   // calculate their respective forces. If the forces match (are within
   // ERROR_LIMIT of each other), stop; else change the length guesses based
   // on the error and try again.
   for (i = 0; i < MAX_ITERATIONS; i++) {
		double activeForce = calcActiveForce(s, fiberLength / optimalFiberLength) * aActivation;
      if( activeForce <  0.0) activeForce = 0.0;

      passiveForce =  calcPassiveForce(s, fiberLength / optimalFiberLength);
      if (passiveForce < 0.0) passiveForce = 0.0;

      fiber_force = (activeForce + passiveForce) * maxIsometricForce * cos_factor;

      norm_tendon_length = tendon_length / optimalFiberLength;
      tendon_force = calcTendonForce(s, norm_tendon_length) * maxIsometricForce;
		setForce(s, tendon_force);

      old_error_force = error_force;
 
      error_force = tendon_force - fiber_force;

      if (DABS(error_force) <= ERROR_LIMIT) // muscle-tendon force found!
         break;

      if (i == 0)
         old_error_force = error_force;

      if (DSIGN(error_force) != DSIGN(old_error_force)) {
         percent = DABS(error_force) / (DABS(error_force) + DABS(old_error_force));
         tmp_fiber_length = old_fiber_length;
         old_fiber_length = fiberLength;
         fiberLength = fiberLength + percent * (tmp_fiber_length - fiberLength);
      } else {
         // Estimate the stiffnesses of the tendon and the fibers. If tendon
         // stiffness is too low, then the next length guess will overshoot
         // the equilibrium point. So we artificially raise it using the
         // normalized muscle force. (_activeForce+_passiveForce) is the
         // normalized force for the current fiber length, and we assume that
         // the equilibrium length is close to this current length. So we want
         // to get force = (_activeForce+_passiveForce) from the tendon as well.
         // We hope this will happen by setting the tendon stiffness to
         // (_activeForce+_passiveForce) times its maximum stiffness.
			double tendon_elastic_modulus = 1200.0;
			double tendon_max_stress = 32.0;

         tendon_stiffness = calcTendonForce(s, norm_tendon_length) *
				maxIsometricForce / tendonSlackLength;

         min_tendon_stiffness = (activeForce + passiveForce) *
	         tendon_elastic_modulus * maxIsometricForce /
	         (tendon_max_stress * tendonSlackLength);

         if (tendon_stiffness < min_tendon_stiffness)
            tendon_stiffness = min_tendon_stiffness;

         fiber_stiffness = maxIsometricForce / optimalFiberLength *
            (calcActiveForce(s, fiberLength / optimalFiberLength)  +
            calcPassiveForce(s, fiberLength / optimalFiberLength));

         // determine how much the fiber and tendon lengths have to
         // change to make the error_force zero. But don't let the
	      // length change exceed half the optimal fiber length because
	      // that's too big a change to make all at once.
         length_change = fabs(error_force/(fiber_stiffness / cos_factor + tendon_stiffness));

         if (fabs(length_change / optimalFiberLength) > 0.5)
            length_change = 0.5 * optimalFiberLength;

         // now change the fiber length depending on the sign of the error
         // and the sign of the fiber stiffness (which equals the sign of
         // the slope of the muscle's force-length curve).
         old_fiber_length = fiberLength;

         if (error_force > 0.0)
             fiberLength += length_change;
         else
             fiberLength -= length_change;


      }
      tendon_length = length - sqrt(fiberLength*fiberLength-_muscleWidth*_muscleWidth);

      // Check to make sure tendon is not shorter than its slack length. If it is,
      // set the length to its slack length and re-compute fiber length.
      if (tendon_length < tendonSlackLength) {
         tendon_length = tendonSlackLength;
         cos_factor = cos(atan(muscle_width / (length - tendon_length)));
         fiberLength = (length - tendon_length) / cos_factor ;
      }
	}

	_model->getMultibodySystem().realize(s, SimTK::Stage::Position);
	setStateVariable(s, STATE_FIBER_LENGTH_NAME,  fiberLength);

//cout << "ThelenMuscle computeIsometricForce " << getName() << "  t=" << s.getTime() << " force = " << tendon_force << endl;

   return tendon_force;
}